import time, thread
from Tkinter import *
import tkFont

import ping
from udp import FieldControlConnection

# Set match period times in seconds
AUTO_TIME = 20
TELEOP_TIME = 120

connection = [None, None, None, None]
teamEntries = []
ipLabels = []
pingLabels = []
responseLabels = [] # List of lists of 3 Labels

modeString = ['Enable', 'Disable', 'Auto', 'Auto/Disable']
modeColor = ['green', 'red', 'blue', 'orange']

matchStartBtn = None
matchContinueBtn = None
timerLabels = None
timerDisplay = None

timerThreadRun = False
def timerThread(timer, callback):
    global timerThreadRun

    if timerThreadRun: return
    timerThreadRun = True
    while timerThreadRun and timer > 0:
        for timerLabel in timerLabels:
            timerLabel.config(text='%d:%02d' % (timer // 60, timer % 60))
        time.sleep(1)
        timer -= 1

    # if thread was stopped, don't call callback
    if not timerThreadRun: return

    for timerLabel in timerLabels:
        timerLabel.config(text='%d:%02d' % (timer // 60, timer % 60))
    timerThreadRun = False
    if callback: callback()

def matchStart():
    def endAuto():
        disableAll()
        matchContinueBtn.config(state=NORMAL)
        matchContinueBtn.flash()

    autoAll()
    matchStartBtn.config(state=DISABLED)
    thread.start_new_thread(timerThread, (AUTO_TIME, endAuto))

def matchContinue():
    def endTeleop():
        disableAll()
        matchReset()

    enableAll()
    matchContinueBtn.config(state=DISABLED)
    thread.start_new_thread(timerThread, (TELEOP_TIME, endTeleop))

def matchReset():
    global timerThreadRun

    disableAll()

    # stop the timer thread
    timerThreadRun = False

    # reset the UI elements
    for timerLabel in timerLabels:
        timerLabel.config(text='--:--')
    matchStartBtn.config(state=NORMAL)
    matchContinueBtn.config(state=NORMAL)


def processResponse(connection, packet, i):
    mode = ord(packet[1])
    masterVer = (ord(packet[2]) << 8) | ord(packet[3])
    userVer = (ord(packet[4]) << 8) | ord(packet[5])

    if mode < 4:
        responseLabels[i][0].config(fg=modeColor[mode], text=modeString[mode])
    else:
        responseLabels[i][0].config(fg='black', text=('Unrecognized (%d)' % mode))
    responseLabels[i][1].config(text=str(masterVer))
    responseLabels[i][2].config(text=str(userVer))

    # Handle nodified packet format (master version >= 4) with battery voltage
    if len(packet) > 6:
        # batt = (0.31177 * battery voltage) / 5 * 1024 
        # scaled to 0-1023 where 1023 is 5.0v
        batt = (ord(packet[6]) << 8) | ord(packet[7])
        batt = batt * 5.0 / 1024.0 / 0.31177
        color = 'green' if batt > 5.4 else 'red'
        responseLabels[i][3].config(fg=color, text=("%.2fv" % batt))

# Callback for updating row i
def updateConnection(i):
    # If team number is blank
    if teamEntries[i].get().strip() == '':
        closeConnection(i)
        return

    # If team number unchanged, do nothing
    teamNo = int(teamEntries[i].get())
    if connection[i] and connection[i].teamNo == teamNo: return

    if connection[i]: connection[i].close()
    connection[i] = FieldControlConnection(teamNo, 
                                           lambda c,p: processResponse(c,p,i))
    connection[i].disable() # set the robot mode to get a response packet
    ipLabels[i].config(text=connection[i].ip)

# Clear row and close connection
def closeConnection(i):
    if connection[i]: connection[i].close()
    connection[i] = None
    ipLabels[i].config(text='-')
    pingLabels[i].config(fg='black', text='-')
    for label in responseLabels[i]: label.config(fg='black', text='-')

def clearAll():
    for i in range(len(teamEntries)):
        teamEntries[i].delete(0,END)
        closeConnection(i)

def pingAll():
    for i in range(len(connection)):
        if connection[i]:
            latency = ping.do_one(connection[i].ip, 0.5)
            if latency:
                pingLabels[i].config(fg='green', text=str(int(latency*1000))+'ms')
            else:
                pingLabels[i].config(fg='red', text='Timeout')
        else:
            pingLabels[i].config(fg='black', text='-')

def updateAll():
    for i in range(len(connection)):
        updateConnection(i)

def enableAll():
    for conn in connection:
        if conn: conn.enable()

def disableAll():
    for conn in connection:
        if conn: conn.disable()

def autoAll():
    for conn in connection:
        if conn: conn.auto()

def disable(i):
    if connection[i]:
        connection[i].disable()

def addStatusRow(root, n):
    index = len(teamEntries)

    entry = Entry(root, width=4)
    entry.grid(row=n, column=0)
    entry.bind('<Return>', lambda e: updateConnection(index))
    teamEntries.append(entry)

    ip = Label(root, text='-', width=10)
    ip.grid(row=n, column=1)    
    ipLabels.append(ip)
    
    ping = Label(root, text='-', width=8)
    ping.grid(row=n, column=2)
    pingLabels.append(ping)


    mode = Label(root, text='-')
    mode.grid(row=n, column=3)

    masterVersion = Label(root, text='-')
    masterVersion.grid(row=n, column=4)

    userVersion = Label(root, text='-')
    userVersion.grid(row=n, column=5)

    battVoltage = Label(root, text='-')
    battVoltage.grid(row=n, column=6)

    disableButton = Button(root, text='Disable',
                           command=lambda: disable(index))
    disableButton.grid(row=n, column=7, sticky=W)

    responseLabels.append([mode, masterVersion, userVersion, battVoltage])

def main():
    def pingAllThread():
        while True:
            pingAll()
            time.sleep(2)  # sleep 2 sec

    root = Tk()
    root.title('PiE Field Control v0.1')

    Label(root, text='Team No').grid(row=0, column=0)
    Label(root, text='IP').grid(row=0, column=1)
    Label(root, text='Ping').grid(row=0, column=2)
    Label(root, text='Robot Mode').grid(row=0, column=3)
    Label(root, text='Master Version').grid(row=0, column=4)
    Label(root, text='User Version').grid(row=0, column=5)
    Label(root, text='Battery').grid(row=0, column=6)

    addStatusRow(root, 1)
    addStatusRow(root, 2)
    addStatusRow(root, 3)
    addStatusRow(root, 4)

    # global control buttons
    controlRow = Frame(root)
    controlRow.grid(row=10, column=0, columnspan=7, sticky=W)

    disable = Button(controlRow, text='Disable', command=disableAll,
                     bg='#f00', activebackground='#ff9999')
    disable.grid(row=0, column=2)
    enable = Button(controlRow, text='Enable', command=enableAll)
    enable.grid(row=0, column=3)
    auto = Button(controlRow, text='Autonomous', command=autoAll)
    auto.grid(row=0, column=4)
    update = Button(controlRow, text='Update', command=updateAll)
    update.grid(row=0, column=5)
    clear = Button(controlRow, text='Reset', command=clearAll)
    clear.grid(row=0, column=6)


    # match control buttons
    compRow = Frame(root)
    compRow.grid(row=12, column=0, columnspan=7, sticky=W)

    global matchStartBtn, matchContinueBtn, timerLabels
    matchStartBtn = Button(compRow, text='Match Start', command=matchStart,
                           bg='#33ff33', activebackground='#99ff99')
    matchStartBtn.grid(row=0, column=1)
    matchContinueBtn = Button(compRow, text='Teleop Start',
                              command=matchContinue)
    matchContinueBtn.grid(row=0, column=2)
    Button(compRow, text='Reset Match', command=matchReset).grid(
        row=0, column=3)
    timerLabel = Label(compRow, text='--:--')
    timerLabel.grid(row=0, column=4, padx=15)

    def toggleDecorate(event):
        timerWindow.overrideredirect(not timerWindow.overrideredirect())

    timerWindow = Toplevel()
    timerWindow.geometry('300x120')
    timerWindow.title('Click window to hide title bar')
    timerWindow.bind('<Button-1>', toggleDecorate)
    timerFont = tkFont.Font(size=52, weight='bold')
    timerDisplay = Label(timerWindow, text='--:--', font=timerFont)
    timerDisplay.pack()

    timerLabels = [timerLabel, timerDisplay]

    # Start a thread to ping robots every few seconds
    thread.start_new_thread(pingAllThread, ())

    root.mainloop()

if __name__ == '__main__':
    main()
